#! /usr/bin/env python
# encoding: utf-8

import rospy
import time
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from sensor_msgs.msg import Joy

# PS3 buttons
X = 0
A = 1
B = 2
Y = 3
LB = 4
RB = 5
LT = 6
RT = 7
BACK = 8
START = 9
LEFT_AXIS = 10
RIGHT_AXIS = 11


def joyCallback(msg):
    if not isinstance(msg, Joy):
        return 0

    axis_linear_x = 1
    axis_angular_z = 2

    vel_linear_x = 0.1
    vel_angular_z = 0.4

    twist = Twist()
    planting = String()

    # linear speed value
    vel_linear_x *= 1 + (msg.axes[5] * 0.1)
    if vel_angular_z >= 0.6:
        vel_linear_x = 0.6
    elif vel_linear_x <= 0:
        vel_linear_x = 0
    else:
        vel_linear_x = vel_linear_x

    # angular speed value
    vel_angular_z *= 1 + (msg.axes[4] * 0.1)
    if vel_angular_z >= 0.6:
        vel_angular_z = 0.6
    elif vel_angular_z <= 0:
        vel_angular_z = 0
    else:
        vel_angular_z = vel_angular_z

    # running
    if msg.buttons[LT]:
        twist.linear.x = msg.axes[axis_linear_x] * vel_linear_x
        twist.linear.y = 0
        twist.linear.z = 0

        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = msg.axes[axis_angular_z] * vel_angular_z

        cmdVelPub.publish(twist)
        rospy.loginfo("linear:{} angular:{}".format(twist.linear.x, twist.angular.z))

    elif not msg.buttons[BACK]:
        # 手动模式
        if not msg.buttons[RT]:
            # forward rotation
            if msg.buttons[X]:
                # head forward rotation
                planting.data = "00"

                plantPub.publish(planting)
                rospy.loginfo("head forward rotation {}".format(planting.data))

            elif msg.buttons[A]:
                # servo forward rotation

                planting.data = "01"
                plantPub.publish(planting)
                rospy.loginfo("servo forward rotation {}".format(planting.data))

            elif msg.buttons[B]:
                # rod forward rotation (elongation)
                planting.data = "02"

                plantPub.publish(planting)
                rospy.loginfo("rod forward rotation (elongation) {}".format(planting.data))

            elif msg.buttons[Y]:
                # step forward rotation
                planting.data = "03"

                plantPub.publish(planting)
                rospy.loginfo("step forward rotation {}".format(planting.data))

        else:
            # inverse rotation
            if msg.buttons[X]:
                # head inverse rotation
                planting.data = "10"

                plantPub.publish(planting)
                rospy.loginfo("head inverse rotation {}".format(planting.data))

            elif msg.buttons[A]:
                # servo inverse rotation
                planting.data = "11"

                plantPub.publish(planting)
                rospy.loginfo("servo inverse rotation {}".format(planting.data))

            elif msg.buttons[B]:
                # rod inverse rotation (shorten)
                planting.data = "12"

                plantPub.publish(planting)
                rospy.loginfo("rod inverse rotation (shorten) {}".format(planting.data))

            elif msg.buttons[Y]:
                # step forward rotation
                planting.data = "13"

                plantPub.publish(planting)
                rospy.loginfo("step inverse rotation {}".format(planting.data))

    elif msg.buttons[BACK]:
        # while not msg.buttons[9]:
        # 未中断
        # startTime = time.perf_counter()
        # while (time.perf_counter() - startTime) <
        pass


def onShutdown():
    rospy.loginfo("stop using teleop key")
    twist = Twist()
    twist.linear.x = 0
    twist.angular.z = 0
    cmdVelPub.publish(twist)


if __name__ == '__main__':
    # ros node
    node_name = "logitech_teleop_key"
    rospy.init_node(node_name)

    rospy.on_shutdown(onShutdown)

    # publisher /cmd_vel
    cmdVelPub = rospy.Publisher("/cmd_vel", Twist, queue_size=59)
    # publisher /plant
    plantPub = rospy.Publisher("/plant", String, queue_size=10)
    # subscriber /joy
    rospy.Subscriber("/joy", Joy, joyCallback)

    # rate
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():

        rate.sleep()
        # rospy.spin()

